import org.lwjgl.*;
import org.lwjgl.opengl.*;
import org.lwjgl.input.*;
import java.nio.*;

public class Frustum {
	
	private float m_Frustum[][] = new float[6][4];
	
	public void NormalizePlane(float frustum[][], int side)
	{
		// Here we calculate the magnitude of the normal to the plane (point A B C)
		// Remember that (A, B, C) is that same thing as the normal's (X, Y, Z).
		// To calculate magnitude you use the equation:  magnitude = sqrt( x^2 + y^2 + z^2)
		float magnitude = (float)Math.sqrt( frustum[side][PlaneData.A.ordinal()] * frustum[side][PlaneData.A.ordinal()] + 
									   frustum[side][PlaneData.B.ordinal()] * frustum[side][PlaneData.B.ordinal()] + 
									   frustum[side][PlaneData.C.ordinal()] * frustum[side][PlaneData.C.ordinal()] );

		// Then we divide the plane's values by it's magnitude.
		// This makes it easier to work with.
		frustum[side][PlaneData.A.ordinal()] /= magnitude;
		frustum[side][PlaneData.B.ordinal()] /= magnitude;
		frustum[side][PlaneData.C.ordinal()] /= magnitude;
		frustum[side][PlaneData.D.ordinal()] /= magnitude; 
	}
	
	public void CalculateFrustum()
	{    
		
		float   proj[] = new float[16];								// This will hold our projection matrix
		float   modl[] = new float[16];								// This will hold our modelview matrix
		float   clip[] = new float[16];								// This will hold the clipping planes
		FloatBuffer temp = BufferUtils.createFloatBuffer(64);
		// glGetFloatv() is used to extract information about our OpenGL world.
		// Below, we pass in GL_PROJECTION_MATRIX to abstract our projection matrix.
		// It then stores the matrix into an array of [16].
		GL11.glGetFloat( GL11.GL_PROJECTION_MATRIX, (FloatBuffer)temp );
		temp.get(proj);
		
		
		// By passing in GL_MODELVIEW_MATRIX, we can abstract our model view matrix.
		// This also stores it in an array of [16].
		GL11.glGetFloat( GL11.GL_MODELVIEW_MATRIX, (FloatBuffer)temp );
		temp.get(modl);
		
		
		// Now that we have our modelview and projection matrix, if we combine these 2 matrices,
		// it will give us our clipping planes.  To combine 2 matrices, we multiply them.

		clip[ 0] = modl[ 0] * proj[ 0] + modl[ 1] * proj[ 4] + modl[ 2] * proj[ 8] + modl[ 3] * proj[12];
		clip[ 1] = modl[ 0] * proj[ 1] + modl[ 1] * proj[ 5] + modl[ 2] * proj[ 9] + modl[ 3] * proj[13];
		clip[ 2] = modl[ 0] * proj[ 2] + modl[ 1] * proj[ 6] + modl[ 2] * proj[10] + modl[ 3] * proj[14];
		clip[ 3] = modl[ 0] * proj[ 3] + modl[ 1] * proj[ 7] + modl[ 2] * proj[11] + modl[ 3] * proj[15];

		clip[ 4] = modl[ 4] * proj[ 0] + modl[ 5] * proj[ 4] + modl[ 6] * proj[ 8] + modl[ 7] * proj[12];
		clip[ 5] = modl[ 4] * proj[ 1] + modl[ 5] * proj[ 5] + modl[ 6] * proj[ 9] + modl[ 7] * proj[13];
		clip[ 6] = modl[ 4] * proj[ 2] + modl[ 5] * proj[ 6] + modl[ 6] * proj[10] + modl[ 7] * proj[14];
		clip[ 7] = modl[ 4] * proj[ 3] + modl[ 5] * proj[ 7] + modl[ 6] * proj[11] + modl[ 7] * proj[15];

		clip[ 8] = modl[ 8] * proj[ 0] + modl[ 9] * proj[ 4] + modl[10] * proj[ 8] + modl[11] * proj[12];
		clip[ 9] = modl[ 8] * proj[ 1] + modl[ 9] * proj[ 5] + modl[10] * proj[ 9] + modl[11] * proj[13];
		clip[10] = modl[ 8] * proj[ 2] + modl[ 9] * proj[ 6] + modl[10] * proj[10] + modl[11] * proj[14];
		clip[11] = modl[ 8] * proj[ 3] + modl[ 9] * proj[ 7] + modl[10] * proj[11] + modl[11] * proj[15];

		clip[12] = modl[12] * proj[ 0] + modl[13] * proj[ 4] + modl[14] * proj[ 8] + modl[15] * proj[12];
		clip[13] = modl[12] * proj[ 1] + modl[13] * proj[ 5] + modl[14] * proj[ 9] + modl[15] * proj[13];
		clip[14] = modl[12] * proj[ 2] + modl[13] * proj[ 6] + modl[14] * proj[10] + modl[15] * proj[14];
		clip[15] = modl[12] * proj[ 3] + modl[13] * proj[ 7] + modl[14] * proj[11] + modl[15] * proj[15];
		
		// Now we actually want to get the sides of the frustum.  To do this we take
		// the clipping planes we received above and extract the sides from them.
		
		// This will extract the RIGHT side of the frustum
		m_Frustum[FrustumSide.RIGHT.ordinal()][PlaneData.A.ordinal()] = clip[ 3] - clip[ 0];
		m_Frustum[FrustumSide.RIGHT.ordinal()][PlaneData.B.ordinal()] = clip[ 7] - clip[ 4];
		m_Frustum[FrustumSide.RIGHT.ordinal()][PlaneData.C.ordinal()] = clip[11] - clip[ 8];
		m_Frustum[FrustumSide.RIGHT.ordinal()][PlaneData.D.ordinal()] = clip[15] - clip[12];

		// Now that we have a normal (A,B,C) and a distance (D) to the plane,
		// we want to normalize that normal and distance.

		// Normalize the FrustumSide.RIGHT side
		NormalizePlane(m_Frustum, FrustumSide.RIGHT.ordinal());

		// This will extract the FrustumSide.LEFT.ordinal() side of the frustum
		m_Frustum[FrustumSide.LEFT.ordinal()][PlaneData.A.ordinal()] = clip[ 3] + clip[ 0];
		m_Frustum[FrustumSide.LEFT.ordinal()][PlaneData.B.ordinal()] = clip[ 7] + clip[ 4];
		m_Frustum[FrustumSide.LEFT.ordinal()][PlaneData.C.ordinal()] = clip[11] + clip[ 8];
		m_Frustum[FrustumSide.LEFT.ordinal()][PlaneData.D.ordinal()] = clip[15] + clip[12];

		// Normalize the FrustumSide.LEFT.ordinal() side
		NormalizePlane(m_Frustum, FrustumSide.LEFT.ordinal());

		// This will extract the FrustumSide.BOTTOM.ordinal() side of the frustum
		m_Frustum[FrustumSide.BOTTOM.ordinal()][PlaneData.A.ordinal()] = clip[ 3] + clip[ 1];
		m_Frustum[FrustumSide.BOTTOM.ordinal()][PlaneData.B.ordinal()] = clip[ 7] + clip[ 5];
		m_Frustum[FrustumSide.BOTTOM.ordinal()][PlaneData.C.ordinal()] = clip[11] + clip[ 9];
		m_Frustum[FrustumSide.BOTTOM.ordinal()][PlaneData.D.ordinal()] = clip[15] + clip[13];

		// Normalize the FrustumSide.BOTTOM.ordinal() side
		NormalizePlane(m_Frustum, FrustumSide.BOTTOM.ordinal());

		// This will extract the FrustumSide.TOP.ordinal() side of the frustum
		m_Frustum[FrustumSide.TOP.ordinal()][PlaneData.A.ordinal()] = clip[ 3] - clip[ 1];
		m_Frustum[FrustumSide.TOP.ordinal()][PlaneData.B.ordinal()] = clip[ 7] - clip[ 5];
		m_Frustum[FrustumSide.TOP.ordinal()][PlaneData.C.ordinal()] = clip[11] - clip[ 9];
		m_Frustum[FrustumSide.TOP.ordinal()][PlaneData.D.ordinal()] = clip[15] - clip[13];

		// Normalize the FrustumSide.TOP.ordinal() side
		NormalizePlane(m_Frustum, FrustumSide.TOP.ordinal());

		// This will extract the FrustumSide.BACK.ordinal() side of the frustum
		m_Frustum[FrustumSide.BACK.ordinal()][PlaneData.A.ordinal()] = clip[ 3] - clip[ 2];
		m_Frustum[FrustumSide.BACK.ordinal()][PlaneData.B.ordinal()] = clip[ 7] - clip[ 6];
		m_Frustum[FrustumSide.BACK.ordinal()][PlaneData.C.ordinal()] = clip[11] - clip[10];
		m_Frustum[FrustumSide.BACK.ordinal()][PlaneData.D.ordinal()] = clip[15] - clip[14];

		// Normalize the FrustumSide.BACK.ordinal() side
		NormalizePlane(m_Frustum, FrustumSide.BACK.ordinal());

		// This will extract the FrustumSide.FRONT.ordinal() side of the frustum
		m_Frustum[FrustumSide.FRONT.ordinal()][PlaneData.A.ordinal()] = clip[ 3] + clip[ 2];
		m_Frustum[FrustumSide.FRONT.ordinal()][PlaneData.B.ordinal()] = clip[ 7] + clip[ 6];
		m_Frustum[FrustumSide.FRONT.ordinal()][PlaneData.C.ordinal()] = clip[11] + clip[10];
		m_Frustum[FrustumSide.FRONT.ordinal()][PlaneData.D.ordinal()] = clip[15] + clip[14];

		// Normalize the FrustumSide.FRONT.ordinal() side
		NormalizePlane(m_Frustum, FrustumSide.FRONT.ordinal());
	}
	
	boolean BoxInFrustum( float x, float y, float z, float x2, float y2, float z2)
	{
		// Go through all of the corners of the box and check then again each plane
		// in the frustum.  If all of them are behind one of the planes, then it most
		// like is not in the frustum.
		for(int i = 0; i < 6; i++ )
		{
			if(m_Frustum[i][PlaneData.A.ordinal()] * x  + m_Frustum[i][PlaneData.B.ordinal()] * y  + m_Frustum[i][PlaneData.C.ordinal()] * z  + m_Frustum[i][PlaneData.D.ordinal()] > 0)  continue;
			if(m_Frustum[i][PlaneData.A.ordinal()] * x2 + m_Frustum[i][PlaneData.B.ordinal()] * y  + m_Frustum[i][PlaneData.C.ordinal()] * z  + m_Frustum[i][PlaneData.D.ordinal()] > 0)  continue;
			if(m_Frustum[i][PlaneData.A.ordinal()] * x  + m_Frustum[i][PlaneData.B.ordinal()] * y2 + m_Frustum[i][PlaneData.C.ordinal()] * z  + m_Frustum[i][PlaneData.D.ordinal()] > 0)  continue;
			if(m_Frustum[i][PlaneData.A.ordinal()] * x2 + m_Frustum[i][PlaneData.B.ordinal()] * y2 + m_Frustum[i][PlaneData.C.ordinal()] * z  + m_Frustum[i][PlaneData.D.ordinal()] > 0)  continue;
			if(m_Frustum[i][PlaneData.A.ordinal()] * x  + m_Frustum[i][PlaneData.B.ordinal()] * y  + m_Frustum[i][PlaneData.C.ordinal()] * z2 + m_Frustum[i][PlaneData.D.ordinal()] > 0)  continue;
			if(m_Frustum[i][PlaneData.A.ordinal()] * x2 + m_Frustum[i][PlaneData.B.ordinal()] * y  + m_Frustum[i][PlaneData.C.ordinal()] * z2 + m_Frustum[i][PlaneData.D.ordinal()] > 0)  continue;
			if(m_Frustum[i][PlaneData.A.ordinal()] * x  + m_Frustum[i][PlaneData.B.ordinal()] * y2 + m_Frustum[i][PlaneData.C.ordinal()] * z2 + m_Frustum[i][PlaneData.D.ordinal()] > 0)  continue;
			if(m_Frustum[i][PlaneData.A.ordinal()] * x2 + m_Frustum[i][PlaneData.B.ordinal()] * y2 + m_Frustum[i][PlaneData.C.ordinal()] * z2 + m_Frustum[i][PlaneData.D.ordinal()] > 0)  continue;

			// If we get here, it isn't in the frustum
			return false;
		}

		// Return a true for the box being inside of the frustum
		return true;
	}
}
